基于力传感器的曲面贴合（按摩）功能开发介绍

[TOC]

此功能可支持机器人运行在位置和速度两种模式。

0 准备

软件通讯性能需满足以下要求：

• 支持速度模式、3型位置模式
• 速度环跟踪精度10rpm左右
• 模式切换耗时1-2个采样周期
• 上位机下发指令到驱动器接收指令耗时在一个采样周期内

软件平台版本不受限制，下表所列仅供参考与验证，相关固件程序链接此处下载

1 接口介绍

具体例程可参考demo

1.1 参数设置

1）创建算法库实例

/**
 * @brief 创建机器人算法库实例
 * @param modelName：机器人对应的 URDF 描述文件
 * @param path: 机器人资源文件的路径
 * @return 算法库实例指针,如果失败, 返回 nullptr
 * ARAL算法库文件可以通过 dlopen 在运行时打开, 通过调用 CreateRLIntfacePtr 函数得到基类接口指针
 */
ARAL::interface::ARALIntfacePtr CreateARALIntfacePtrFromFile(const char* model_name, const char* path);

2）设置控制器模式，此功能需设置为ADM

    /**
     * @brief 设置控制类型
     * @param type:主要有三种类型: 1）位置控制; 2）导纳控制; 3）阻抗控制
     *        主类型下面又分一些子类型, 如导纳控制目前支持 0:拖动示教; 1:轨迹跟踪(柔顺); 2:恒力跟踪等功能
     *        子类型无需用户显式指定, 而是根据用户设置的不同参数调整算法控制器结构来实现对应的功能
     * @return 返回设置状态
     */
    ARAL_API_COMMON(1.0) int mcSetControlType(const ControlMode& type) = 0;

3）初始化机器人状态

    /**
     * @brief 初始化算法库内部机械臂的状态
     *  切换控制器之前, 需要先调用这个函数
     */
    ARAL_API_COMMON(1.0) int rsInitiateRobotState() = 0;

4）设置控制周期5ms

    /**
     * @brief 设置控制算法周期, 如不设置, 则默认取值为(0.005s)
     * @param period: 控制周期, 单位（s）
     * @return: 返回值 < 0 表示设置失败
     */
    ARAL_API_COMMON(1.0) int mcSetControlPeriod(const double period) = 0;

5）设置力控方向，空间选择为CARTESIAN

    /**
     * @brief 设置力控选择方向向量
     * @param space: 空间选择, 取值 JOINT 或 CARTESIAN, 其他非法
     * @param value: 方向向量, 元素取值0为关闭对应维度力控, 1为开启对应维度力控;
     *               长度为工作空间维度或机器人自由度，顺序为[x,y,z,Rx,Ry,Rz]或[J1,J2,J3,J4,J5,J6]
     * @return 返回值 < 0 表示设置失败
     */
    ARAL_API_COMMON(1.0) int fcSetSelectionVector(const DescribeSpace& space, const int* value) = 0;

6）设置力阈值，空间选择为CARTESIAN

    /**
     * @brief 设置控制算法对输入的响应阈值
     * @param space: 空间选择, 取值 JOINT 或 CARTESIAN, 其他非法
     * @param threshold:阈值, 笛卡尔空间单位N、Nm，长度为工作空间维度, 顺序为[x,y,z,Rx,Ry,Rz]
     *                  关节空间单位Nm，长度为机器人自由度，顺序为[J1,J2,J3,J4,J5,J6]
     *                  当作用于控制算法的数值超过该值时，算法才有效。(该值为非负值)
     * @return 返回值 < 0 表示设置失败
     */
    ARAL_API_COMMON(1.0) int fcSetForceThreshold(const DescribeSpace& space, const double* threshold) = 0;

7）设置力限制，空间选择为CARTESIAN

    /**
     * @brief 设置控制算法对输入的响应限制值
     * @param space: 空间选择, 取值 JOINT 或 CARTESIAN, 其他非法
     * @param data: 限制值，如果超过该值时，会截断数据，按照最大值计算。(该值为正值)
     *             笛卡尔空间单位N、Nm，长度为工作空间维度（一般是6），顺序为[x,y,z,Rx,Ry,Rz]
     *             关节空间单位Nm，长度为机器人自由度（一般是6），顺序为[J1,J2,J3,J4,J5,J6]
     * @return 返回值 < 0 表示设置失败
     */
    ARAL_API_COMMON(1.0) int fcSetForceLimit(const DescribeSpace& space, const double* data) = 0;

8）设置力控设计参数，具体由算法部提供参数范围(该接口准备废弃，后续由 fcCalForceControlMDKPara 代替)

    /**
     * @brief 设置力控参数作用于力控算法
     * @param space: 空间选择
     * @param para: 设计参数，笛卡尔空间长度为工作空间维度（一般是6），顺序为[x,y,z,Rx,Ry,Rz]
     *               关节空间长度为机器人自由度（一般是6），顺序为[J1,J2,J3,J4,J5,J6]
     */
    ARAL_API_COMMON(1.0) int fcSetForceControlParams(const interface::DescribeSpace& space, const interface::FcDesignParam& para) = 0;

8）设置力控控制参数MDK，space空间选择为CARTESIAN

    /**
     * @brief 设置阻抗控制质量参数, 参数的数值必须是正数
     * @param space: 空间选择, 取值 JOINT 或 CARTESIAN, 其他非法
     * @param data: 质量，笛卡尔空间单位kg，长度为工作空间维度（一般是6），顺序为[x,y,z,Rx,Ry,Rz]
     *             关节空间单位kg*m^2，长度为机器人自由度（一般是6），顺序为[J1,J2,J3,J4,J5,J6]
     *             质量数值越大，相同加速度产生的阻力/力矩越大。
     * @return: 返回设置状态
     */
    ARAL_API_COMMON(1.0) int fcSetMass(const DescribeSpace& space, const double* mass) = 0;

    /**
     * @brief 设置阻抗控制阻尼参数, 参数的数值必须是正数
     * @param space: 空间选择, 取值 JOINT 或 CARTESIAN, 其他非法
     * @param damp: 阻尼，笛卡尔空间单位N*s/m，长度为工作空间维度（一般是6），顺序为[x,y,z,Rx,Ry,Rz]
     *             关节空间单位Nm*s/rad，长度为机器人自由度（一般是6），顺序为[J1,J2,J3,J4,J5,J6]
     *             阻尼数值越大，相同速度产生的阻力/力矩越大。
     * @return: 返回设置状态
     */
    ARAL_API_COMMON(1.0) int fcSetDamp(const DescribeSpace& space, const double* damp) = 0;

   /**
     * @brief 设置阻抗控制刚度参数, 参数的数值必须是正数
     * @param space: 空间选择, 取值 JOINT 或 CARTESIAN, 其他非法
     * @param stiffness: 刚度，笛卡尔空间单位N/m，长度为工作空间维度（一般是6），顺序为[x,y,z,Rx,Ry,Rz]
     *                  关节空间单位Nm/rad，长度为机器人自由度（一般是6），顺序为[J1,J2,J3,J4,J5,J6]
     *                  刚度数值越大，力控参考坐标系距离参考位置越远，产生的力/力矩越大。
     * @return: 返回设置状态
     */
    ARAL_API_COMMON(1.0) int fcSetStiffness(const DescribeSpace& space, const double* stiffness) = 0;

质量参数，表示所期望力控对象的质量(平移)或惯量(旋转)大小，与机器人型号和负载相关，例如经验值aubo_i5一般取M={20，20，20，5，5，5}，aubo_i10取M={30，30，30，10，10，10}

阻尼参数，表示所期望力控对象整体的阻尼大小，具体表现为机械臂在自由状态时(不受力)可粗略估计为单位大小的目标力与实际运动速度的比值关系，该值越大力控响应越慢，但越小越容易引起机械臂抖动。同时与质量参数相关，一般D/M比值在[20, 30]区间内

刚度参数，表示所期望力控对象看作弹簧的劲度系数，刚度越大机械臂越“硬”，偏离所设置的参考轨迹(即平衡位置)越困难。曲面贴合（按摩）功能中力控方向的刚度为0

9）设置传感器位姿

    /**
     * @brief 设置末端传感器坐标系在机器人法兰坐标系下的位姿
     * @param pose: 位置和姿态数组信息
     * @return if < 0, 表示设置失败
     */
    ARAL_API_COMMON(1.0) int mdlSetEndSensorPoseInFlange(const RLPose& pose) = 0;

10）设置工具末端位姿

    /**
     * @brief 设置工具坐标系的位姿(调用此函数会触发规划初始点状态的更新)
     *      1. 如果是末端工具，则为 tcp 相对于法兰坐标系的位姿
     *      2. 如果是远端工具，则为 tcp 相对于基坐标系的位姿, 需要先调用　mdlEnableRemoteTool　来使能远端工具
     * @param pose: 位置和姿态数组信息
     * @return if < 0, 表示设置失败
     */
    ARAL_API_COMMON(1.0) int mdlSetToolPose(const RLPose& pose) = 0;

1.2 工具标定

负载信息可选择使用CAD模型或三点标定法

1）若使用CAD模型，需提供负载质量、质心及传感器偏置等信息；

2）若使用三点标定法，调用以下接口

   /**
     * @brief 根据末端 6 维力/力矩传感器的信息标定工具和传感器的属性( 3 点法), 只能标定工具的质量和质心在法兰坐标系的描述
     * @param T_b_f: 输入多种不同构型对应的法兰在基坐标系的描述, 这几种构型的姿态差异越大越好（个数应不小于 3）
     * @param mesureData: 在对应构型下测得的传感器的原始数据
     * @param res: 标定结果(见参考文档)
     * @return 返回值 < 0, 表示计算失败
     */
    ARAL_API_COMMON(1.0) int calibToolDynamicParameterWithFTSensor(const std::vector<RLPose>& T_b_f, const std::vector<RLWrench>& mesureData, FtSensorCalibResult& res) = 0;

负载惯量信息均设为0，最后调用外设模块peripheral的FTSensorOffset()接口设置传感器偏置，调用以下接口设置负载动力学参数

    /**
     * @brief 设置负载的动力学参数, 参考坐标系为末端法兰坐标系
     * @param inertial: 负载的动力学参数结构体
     * @return < 0, 表示设置失败
     */
    ARAL_API_COMMON(1.0) int mdlSetLoadDynamicParameterInFlange(const RLInertia& inertia) = 0;

1.3 生成参考轨迹

参考轨迹可由用户给定或者示教特征点后调用算法库关节B样条功能生成

无论参考轨迹以哪种形式给定，需要保证给定的参考轨迹与接触面贴合的尽量好。相同条件下，参考轨迹越贴合接触面，力控效果越好。

关节B样条功能接口用法如下，输入为示教点的关节角，输出为一条离线轨迹(关节角)

  //! 更新规划起始点
  RLJntArray tmp = { 0, 0, 0, 0, 0, 0 };
  aral->tpInitiatePlanner(q_start, tmp, tmp, PlannerStatus::IDEL);   // q_start为起始点关节角

  //! 向规划器中添加路点
  PathProperty path_property;
  MoveProperty move_property;
  std::vector<PathPoint> points;
  int ret;
  move_property.moveMode = MoveMode::POSITION; //位置模式
  move_property.maxV = { 3.11049, 3.11049, 3.11049, 3.11018, 3.11018, 3.11018 };
  move_property.maxA = { 31.1049, 31.1049, 31.1049, 20, 20, 20};
  move_property.maxJ = { 248.839, 248.839, 248.839, 160, 160, 160};

  double period = 2.0;   // 为两个示教特征点的时间间隔，调整此值可调节速度大小
  points.resize(point_sz - 1); // 只有N-1个目标点，point_sz为示教的特征点个数

  for (unsigned int i = 0; i < point_sz - 1; i++) {
       memcpy(points[i].joint.data(), point[i + 1].data(), sizeof(double) * ROBOT_DOF);

       points[i].type = DescribeSpace::JOINT;
       if (move_property.moveTimes.size() == 0)
           move_property.moveTimes.push_back(period);
       else
           move_property.moveTimes.push_back(period + move_property.moveTimes.back());
  }
  move_property.moveDuration = move_property.moveTimes.back();

  path_property.describe_space = DescribeSpace::JOINT;
  path_property.blend_radius = { -1, -1 }; //交融半径设置相同
  path_property.curProp.type = CurveType::BSPLINE;
  ret = aral->tpAddPoints(points, path_property, move_property);

  if (ret < 0) {
     std::cout << "向规划器中添加直线运动失败！" << std::endl;
     return -1;
  }

  //! 得到规划器在某个时刻的轨迹点
  TrajectoryPoint offline_points;
  int retval = 0;
  do {
     retval = aral->tpUpdateCycle(DT, offline_points);
     if (retval < 0)
         return -1;

     //! 保存离线轨迹
     offline_traj_q.push_back(offline_points.point.joint);
     offline_traj_qd.push_back(offline_points.qd);
     offline_traj_qdd.push_back(offline_points.qdd);
     } while (retval != E_PLN_EXEC_EMPTY);

至此，得到离线轨迹(关节角、关节角速度、关节角加速度)

1.4 运动至离线轨迹第一个点

注意！

开启力控前机器人必须运动至离线轨迹第一个点，即机器人需要运动到与接触面接近的某个位置（例如接触面上方小于10mm的位置，这个用户可根据经验调整），以保证从非接触到接触的过渡过程超调较小。

1.5 开启力控

本步骤为实时接口，循环调用，尽量避免耗时操作

1）更新机器人运动状态

    /**
     * @brief 更新机器人关节的实时状态信息。(如果用户不能给出全部信息, 可以先调用 utlKalmanFilterPVA 函数进行估计)
     *        调用这个接口会连带更新机械臂所有连杆在笛卡尔空间的运动学信息, 包括位姿, 速度和加速度等
     * @param q: 关节当前位置
     * @param qd: 关节当前速度
     * @param qdd: 关节当前加速度
     * @param　is_user_acc: 是否采用用户输入的加速度(默认是否)
     * @return if < 0, 表示设置失败
     */
    ARAL_API_COMMON(1.0) int rsUpdateJointPVA(const RLJntArray& q, const RLJntArray& qd, const RLJntArray& qdd, const bool& is_user_acc = false) = 0;

2）更新传感器数据

    /**
     * @brief 更新力传感器的信息, 处于力控模式时每个周期都需更新(传感器的偏置需要在软件层处理)
     * (！！！在使用时先更新机械臂的状态, 再更新传感器的状态)
     * @param type: END_FT_SENSOR 或者 BASE_FT_SENSOR 或者 JOINT_FT_SENSOR
     * @param ftData: 传感器在每个维度方向的测量数据
     * @return if < 0, 表示设置失败
     */
    ARAL_API_COMMON(1.0) int rsUpdateFTSensorData(const FTSensorType& type, const double* ftData) = 0;

3）设置目标力，此功能控制坐标系类型space为USER

    /**
     * @brief 设置机械臂在控制坐标系下的目标力/力矩(如果是动态坐标系或者时变力,则每个控制周期都需要设置)
     * @param space: 控制坐标系类型
     * @param pose: 如果space不是关节空间,则表示控制坐标系相对基坐标系的相位位姿
     * @param force: 目标力/力矩(一般在笛卡尔空间描述, 在关节空间不存在目标输出力矩)
     * @return: if < 0, 表示计算失败
     */
    ARAL_API_COMMON(1.0) int rsSetGoalForce(const CoordType& space, const RLPose& frame, const RLWrench& force) = 0;

4）设置参考轨迹

    /**
     * @brief 设置参考轨迹(用户设置参考位置时，必须同时设置参考速度和参考加速度;否则可以都设置为空)
     * @param type: 参考轨迹描述坐标系,具体可参考CoordType定义
     * @param frame: 如果type不等于JOINT, 则frame表示参考轨迹坐标系相对基坐标系的相对位姿关系
     * @param positions: 参考轨迹的位置信息, 如没有, 则置空
     * @param velocities: 参考轨迹的速度信息, 如没有, 则置空
     * @param accelerations: 参考轨迹的加速度信息, 如没有, 则置空
     * @return if < 0, 则表示设置失败
     */
    ARAL_API_COMMON(1.0) int rsSetReferenceTrajectory(const CoordType& type, const RLPose& frame, const double* positions,
                                                      const double* velocities, const double* accelerations) = 0;

5）计算控制输出

    /**
     * @brief 计算关节的控制指令参数.(调用该函数时, 如果算法出错, 则返回数据不可用)
     * @param res: 返回值, 如果是位置控制, 则表示关节角度; 如果是力矩控制,  则表示关节电流
     * @param qd:  返回值, 如果是位置控制, 则表示关节指令速度; 否则无意义
     * @param qdd: 返回值, 如有是位置控制, 则表示关节指令加速度; 否则无意义
     * @param ik_eps: 逆解迭代精度参数
     * @return return: 0 - 计算正确
     *            -60003 - 运动速度超限，解决方法：调整力控参数
     *             65001 - 机器人没有参考轨迹，解决方法：调用rsSetReferenceTrajectory设置参考轨迹
     *            -30027 - 没有和参考角在同一关节子空间的逆解，解决方法：未实现在子空间内搜索逆解, 建议更改选解条件，RobotConfiguration::NONE
     *            -30006 - IK 计算失败；解决方法：机械臂在当前位置没有解，且没有使能迭代解,　建议使能迭代解
     *            -40055 - 速度规划失败，解决方法：检查始末速度是否超过运动约束、检查运动约束是否大于0
     */
    ARAL_API_COMMON(1.0) int rsCalJointCommand(RLJntArray& res, RLJntArray& qd, RLJntArray& qdd, const Vector2d& ik_eps = {ARAL_IK_ITER_POS_EPS, ARAL_IK_ITER_ORI_EPS}) = 0;